
#include <winsock2.h>
#include "sma_frame.h"
#include "crc32.h"
#include "sma_em.h"

/* Maximum size of data field of command frame = 32 byte (data for command 0x04:0x51) */
#define EM_MAX_COMMAND_DATA_SIZE  32
/* Maximum size of data field of reply frame = 669 byte (data for command 0x02:0x21) */
#define EM_MAX_REPLY_DATA_SIZE    669

/* Maximum size of command frame = 21 byte (header & crc) + max command data size */
#define EM_MAX_COMMAND_FRAME_SIZE  (21 + EM_MAX_COMMAND_DATA_SIZE)
/* Maximum size of reply frame = 21 byte (header & crc) + max reply data size */
#define EM_MAX_REPLY_FRAME_SIZE    (21 + EM_MAX_REPLY_DATA_SIZE)

typedef struct _SmaEmInfo
{
	HANDLE handle_mutex;
	HANDLE handle_thread;
	WSADATA wsa_data;
	SOCKET listen_socket;
	char command_buff[EM_MAX_COMMAND_FRAME_SIZE];
	char reply_buff[EM_MAX_REPLY_FRAME_SIZE];
	unsigned char command_data[EM_MAX_COMMAND_DATA_SIZE];
	unsigned char reply_data[EM_MAX_REPLY_DATA_SIZE];
} SmaEmInfo;

/* ---------------------------------------------------------------------- */

static DWORD WINAPI sma_em_thread(LPVOID param);
static int emClientHandler(SOCKET client_socket);

static int emProcessFrame(
	const unsigned char *command_buff, 
	unsigned long command_len, 
	unsigned char *reply_buff, 
	unsigned long reply_size, 
	unsigned long *reply_len);

static int emProcessCommand(
	unsigned char cmd_type, 
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessMandatoryCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessTelemetryCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessTeleopCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessDrillScopeCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessPowerManageCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessEsmNop(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessEsmSafe(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessExitSafe(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessEsmReset(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessEsmAbort(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessEsmGetSwVersion(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessEsmSelfTest(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessInquireTestProgress(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessGetTelemetryMcp(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessTeleopArmJoint(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessTeleopArmEe(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessMoveArmToArp(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessHomeArm(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessStowArm(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessChangeDrillTool(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessStartDrillTarget(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessStartObserveTarget(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessNotifyTask(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessManagePower(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static int emProcessRegulateCameraLight(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data);

static SmaEmInfo em_info;

/* ---------------------------------------------------------------------- */

int emInit(void)
{
	int ret;

	generate_crc_table();
		
	em_info.handle_mutex = CreateMutex(NULL, FALSE, NULL);
	if(em_info.handle_mutex == NULL)
	{
		return -1;
	}

	ret = WSAStartup(MAKEWORD(2, 2), &em_info.wsa_data);
	if(ret != 0)
	{
		CloseHandle(em_info.handle_mutex);
		return -1;
	}

	return 0;
}

int emDeinit(void)
{
	int ret;
	int error;

	error = 0;

	ret = WSACleanup();
	if(ret != 0)
	{
		error = 1;
	}

	CloseHandle(em_info.handle_mutex);

	return error ? -1 : 0;
}

int emStart(void)
{
	em_info.handle_thread = CreateThread(
		NULL,
		0,
		sma_em_thread,
		NULL,
		0,
		NULL);
	if(em_info.handle_thread == NULL)
	{
		return -1;
	}
	printf("CreateThread.\n");
	return 0;
}

int emStop(void)
{
	WaitForSingleObject(em_info.handle_thread, INFINITE);
	CloseHandle(em_info.handle_thread);

	return 0;
}

/* ---------------------------------------------------------------------- */

static DWORD WINAPI sma_em_thread(LPVOID param)
{
	int ret;
	struct sockaddr_in sock_address;
	SOCKET client_socket;

	printf("sma_em_thread loaded. \n");

	em_info.listen_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if(em_info.listen_socket == INVALID_SOCKET)
	{
		return -1;
	}

	memset(&sock_address, 0, sizeof(sock_address));
	sock_address.sin_family = AF_INET;
	sock_address.sin_addr.s_addr = INADDR_ANY;
	sock_address.sin_port = htons(12345);

	ret = bind(em_info.listen_socket, (struct sockaddr *)&sock_address, sizeof(sock_address));
	if(ret != 0)
	{
		closesocket(em_info.listen_socket);
		return -1;
	}
	printf("listen started.\n");
	ret = listen(em_info.listen_socket, 5);
	if(ret != 0)
	{
		closesocket(em_info.listen_socket);
		return -1;
	}
	printf("socket createded. \n");

	while(1)
	{
		printf("waiting client_socket. \n");
		client_socket = accept(em_info.listen_socket, NULL, NULL);
		if(client_socket == INVALID_SOCKET)
		{
			break;
		}
		printf("client_socket %d \n.",client_socket);
		ret = emClientHandler(client_socket);
		if(ret < 0)
		{
			continue;
		}
	}

	closesocket(em_info.listen_socket);

	return 0;
}

static int emClientHandler(SOCKET client_socket)
{
	int ret;
	int error;
	char *command_buff;
	int command_size;
	int command_len;
	char *reply_buff;
	int reply_size;
	int reply_len;

	error = 0;

	command_buff = em_info.command_buff;
	command_size = EM_MAX_COMMAND_FRAME_SIZE;

	reply_buff = em_info.reply_buff;
	reply_size = EM_MAX_REPLY_FRAME_SIZE;
	printf("start waiting data.\n");

	while(1)
	{
		ret = recv(client_socket, command_buff, command_size, 0);
		printf("r");
		if(ret == SOCKET_ERROR)
		{
			error = 1;
			break;
		}
		else if(ret == 0)
		{
			/* Connection closed. */
			break;
		}

		command_len = ret;
		//printf("recv code%i\n",ret);

		ret = emProcessFrame(
			(unsigned char *)command_buff, 
			command_len, 
			(unsigned char *)reply_buff, 
			reply_size, 
			(unsigned long *)&reply_len);
		if(ret < 0)
		{
			continue;
		}

		ret = send(client_socket, reply_buff, reply_len, 0);
		printf("s");
		if(ret == -1)
		{
			error = 1;
			break;
		}
	}

	shutdown(client_socket, SD_BOTH);

	closesocket(client_socket);

	return error ? -1 : 0;
}

static int emProcessFrame(
	const unsigned char *command_buff, 
	unsigned long command_len, 
	unsigned char *reply_buff, 
	unsigned long reply_size, 
	unsigned long *reply_len)
{
	int ret;
	SmaFrameHeader header;
	unsigned char *recv_data;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char *send_data;
	unsigned long send_size;
	unsigned long send_len;
	unsigned long sent_time;
	unsigned char dst_id;

	recv_data = em_info.command_data;
	recv_size = EM_MAX_COMMAND_DATA_SIZE;

	send_data = em_info.reply_data;
	send_size = EM_MAX_REPLY_DATA_SIZE;

	ret = smaUnpackDataFrame(command_len, command_buff, &header, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		return -1;
	}

	if(header.dst_id != DEVICE_ID_MCP)
	{
		/* Incorrect destination. */
		return -1;
	}

	ret = emProcessCommand(header.cmd_type, header.cmd_code, recv_len, recv_data, send_size, &send_len, send_data);
	if(ret < 0)
	{
		return -1;
	}

	dst_id = header.src_id;
	sent_time = GetTickCount();

	header.src_id = DEVICE_ID_MCP;
	header.dst_id = dst_id;
	header.sent_time = sent_time;
	header.cmd_type |= 0x80; /* Set bit7 to 1 to indicate reply frame. */

	ret = smaPackDataFrame(reply_size, reply_buff, &header, send_len, send_data);
	if(ret < 0)
	{
		return -1;
	}

	*reply_len = smaCalculateFrameSize(send_len);

	return 0;
}

static int emProcessCommand(unsigned char cmd_type, 
							unsigned short cmd_code, 
							unsigned long recv_len,
							const unsigned char *recv_data,
							unsigned long send_size,
							unsigned long *send_len,
							unsigned char *send_data)
{
	int ret;

	ret = -1;

	switch(cmd_type)
	{
	case 0x01:
		ret = emProcessMandatoryCommand(cmd_code, recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x02:
		ret = emProcessTelemetryCommand(cmd_code, recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x03:
		ret = emProcessTeleopCommand(cmd_code, recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x04:
		break;

	case 0x05:
		ret = emProcessDrillScopeCommand(cmd_code, recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x06:
		ret = emProcessPowerManageCommand(cmd_code, recv_len, recv_data, send_size, send_len, send_data);
		break;

	default:
		break;
	}

	return ret;
}

static int emProcessMandatoryCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	int ret;

	ret = -1;

	switch(cmd_code)
	{
	case 0x01:
		ret = emProcessEsmNop(recv_len, recv_data, send_size, send_len, send_data);
		break;
		
	case 0x02:
		ret = emProcessEsmSafe(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x03:
		ret = emProcessExitSafe(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x04:
		ret = emProcessEsmReset(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x05:
		ret = emProcessEsmAbort(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x06:
		ret = emProcessEsmGetSwVersion(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x07:
		break;

	case 0x08:
		ret = emProcessEsmSelfTest(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x09:
		ret = emProcessInquireTestProgress(recv_len, recv_data, send_size, send_len, send_data);
		break;

	default:
		break;
	}

	return ret;
}

static int emProcessTelemetryCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	int ret;

	ret = -1;

	switch(cmd_code)
	{
	case 0x21:
		ret = emProcessGetTelemetryMcp(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x22:
		break;

	default:
		break;
	}

	return ret;
}

static int emProcessTeleopCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	int ret;

	ret = -1;

	switch(cmd_code)
	{
	case 0x31:
		ret = emProcessTeleopArmJoint(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x32:
		ret = emProcessTeleopArmEe(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x33:
		ret = emProcessMoveArmToArp(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x34:
		ret = emProcessHomeArm(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x35:
		ret = emProcessStowArm(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x36:
		ret = emProcessChangeDrillTool(recv_len, recv_data, send_size, send_len, send_data);
		break;

	default:
		break;
	}

	return ret;
}

static int emProcessDrillScopeCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	int ret;

	ret = -1;

	switch(cmd_code)
	{
	case 0x71:
		ret = emProcessStartDrillTarget(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x72:
		ret = emProcessStartObserveTarget(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x73:
		ret = emProcessNotifyTask(recv_len, recv_data, send_size, send_len, send_data);
		break;

	default:
		break;
	}

	return ret;
}

static int emProcessPowerManageCommand(
	unsigned short cmd_code, 
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	int ret;

	ret = -1;

	switch(cmd_code)
	{
	case 0x91:
		ret = emProcessManagePower(recv_len, recv_data, send_size, send_len, send_data);
		break;

	case 0x92:
		ret = emProcessRegulateCameraLight(recv_len, recv_data, send_size, send_len, send_data);
		break;

	default:
		break;
	}

	return ret;
}

static int emProcessEsmNop(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessEsmSafe(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessExitSafe(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessEsmReset(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessEsmAbort(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessEsmGetSwVersion(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	char ver[] = "SMA MCP 1.00";

	if(send_size < 12)
	{
		return -1;
	}

	memcpy(send_data, ver, 12);
	*send_len = 12;

	return 0;
}

static int emProcessEsmSelfTest(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessInquireTestProgress(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessGetTelemetryMcp(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 669)
	{
		return -1;
	}

	memset(send_data, 1, 669);
	*send_len = 669;

	return 0;
}

static int emProcessTeleopArmJoint(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(recv_len < 6)
	{
		return -1;
	}

	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessTeleopArmEe(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(recv_len < 26)
	{
		return -1;
	}

	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessMoveArmToArp(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessHomeArm(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessStowArm(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessChangeDrillTool(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessStartDrillTarget(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(recv_len < 4)
	{
		return -1;
	}

	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessStartObserveTarget(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(recv_len < 1)
	{
		return -1;
	}

	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessNotifyTask(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(recv_len < 3)
	{
		return -1;
	}

	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessManagePower(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(recv_len < 2)
	{
		return -1;
	}

	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

static int emProcessRegulateCameraLight(
	unsigned long recv_len,
	const unsigned char *recv_data,
	unsigned long send_size,
	unsigned long *send_len,
	unsigned char *send_data)
{
	if(recv_len < 1)
	{
		return -1;
	}

	if(send_size < 1)
	{
		return -1;
	}

	send_data[0] = 1;
	*send_len = 1;

	return 0;
}

/* ---------------------------------------------------------------------- */


